package nape.dynamics;
import cx.Allocator;
import cx.Algorithm;
import nape.Config;
import nape.Const;
import nape.util.FastMath;
import nape.dynamics.Arbiter_false_false_true_true_ShapeShape;
import nape.dynamics.Arbiter_false_false_true_false_Shape_Particle;
import nape.dynamics.Arbiter_true_true_true_true_ShapeShape;
import nape.dynamics.Arbiter_true_true_true_false_Shape_Particle;


















class Collide {
	
	private var alloc:Allocator;
	
	
	
	public function new(ALLOC:Allocator) alloc = ALLOC
	
	
	
	public inline function polyAxisProject(a:Axis, s:Polygon) {
		var min = Const.FMAX;
		CxIter(v,s.pverts,{
			var k = vec_dot(a.n,v.p);
			if (k<min) min = k;
		});
		return min-a.d;
	}
	public inline function polyContains(s:Polygon, p:Vec2) {
		return CxForAll(a,s.paxi, vec_dot(a.n,p.p)<=a.d );
	}
	public inline function circleContains(c:Circle, p:Vec2 ) {
		vec_new(d); vec_sub(p.p,c.centre.p,d);
		return vec_lsq(d)<c.r*c.r;
	}
	public inline function partContains(t:Particle, p:Vec2 ) {
		vec_new(d); vec_sub(p.p,t.p,d);
		return vec_lsq(d)<Config.PARTICLE_RADIUS*Config.PARTICLE_RADIUS;
	}
	public inline function polyPartContains(s:Polygon, p:Vec2, nx:Float, ny:Float) {
		var ret = true;
		CxIter(a,s.paxi,{
			if(vec_dot(a.n,n)<0.0) cxcontinue;
			if(vec_dot(a.n,p.p) > a.d) {
				ret = false;
				break;
			}
		});
		return ret;
	}
	
	
	
	public inline function bodyContains(b:Body,p:Vec2) {
		return CxExists(s,b.shapes,
			if(s.type==Shape.CIRCLE) circleContains(s.circle, p)
			else                     polyContains  (s.polygon,p)
		);
	}
	
	
	
	
	
	
	
	
	
	
	
	
	private inline function circle2circle_test(c1:Circle,c2:Circle) {
		return circle2circle_query_test(c1.centre,c2.centre,c1.r,c2.r);
	}
	
	
	private inline function circle2circle_query_test(p1:Vec2,p2:Vec2,r1:Float,r2:Float) {
		var minDist = r1+r2;
		vec_new(p); vec_sub(p2.p,p1.p,p);
		var distSqr = vec_lsq(p);
		return distSqr < minDist*minDist;
	}
	
	
	
	
	
	private inline function poly2poly_test(p1:Polygon,p2:Polygon) {
		var cont = true;
		
		var max1  = -Const.FMAX;
		var axis1:Axis = null;
		CxIter(ax,p1.paxi,{
			var min = polyAxisProject(ax,p2);
			if (min>0) { cont = false; break; }
			if (min>max1) {
				max1  = min;
				axis1 = ax;
			}
		})
	;
		if(cont) {
			
		var max2  = -Const.FMAX;
		var axis2:Axis = null;
		CxIter(ax,p2.paxi,{
			var min = polyAxisProject(ax,p1);
			if (min>0) { cont = false; break; }
			if (min>max2) {
				max2  = min;
				axis2 = ax;
			}
		})
	;
			return cont;
		}else return false;
	}
	
	
	
	
	
	
	private inline function circle2poly_test(circle:Circle,poly:Polygon) {
		var a0 = null, vi = null;
		var cont = true;
		var max = -Const.FMAX;
		
		var v = poly.pverts.begin();
		CxIter(a,poly.paxi,{
			var dist = vec_dot(a.n,circle.centre.p) - a.d - circle.r;
			if (dist>0) { cont = false; break; }
			if (dist>max) {
				max = dist;
				a0 = a;
				vi = v;
			}
			v = v.next;
		});
		
		if(cont) {
			var v0 = vi.elem();
			var v1 = if(vi.next==poly.pverts.end()) poly.pverts.begin().elem(); else vi.next.elem();
			
			var dt = vec_cross(a0.n,circle.centre.p);
			if      (dt <  vec_cross(a0.n,v1.p)) return circle2circle_query_test(circle.centre, v1, circle.r, 0);
			else if (dt >= vec_cross(a0.n,v0.p)) return circle2circle_query_test(circle.centre, v0, circle.r, 0);
			else    return true;
		}else return false;
	}
	
	
	
	
	private inline function particleQuery_test(p1:Vec2,part:Particle,r1:Float) {
		var minDist = r1 + Config.PARTICLE_RADIUS;
		vec_new(p); vec_sub(part.p,p1.p,p);
		var distSqr = vec_lsq(p);
		return distSqr < minDist*minDist;
	}
	
	
	private inline function circle2particle_test(circ:Circle,part:Particle) {
		return particleQuery_test(circ.centre,part,circ.r);
	}
	
	
	
	
	private inline function poly2particle_test(poly:Polygon,part:Particle) {
		var max  = -Const.FMAX;
		var axis = null;
		var cont = true;
		CxIter(a,poly.paxi,{
			var min = vec_dot(a.n,part.p) - a.d - Config.PARTICLE_RADIUS;
			if (min>0) { cont = false; break; }
			if (min>max) {
				max  = min;
				axis = a;
			}
		});
		return cont;
	}
}



class RayCast {
	
	static public inline var FAIL:Float = 10.0; 
	static public inline function rayCircle(r:Ray,c:Circle) {
		vec_new(ac); vec_sub(r.a,c.centre.p,ac);
		var A = vec_lsq(r.v);
		var B = 2*vec_dot(r.v,ac);
		var C = vec_lsq(ac) - c.r*c.r;
		
		var D = B*B-4*A*C;
		if(D>=0) {
			D = FastMath.sqrt(D);
			A = 1/(2*A);
			
			var t = (-B-D)*A;
			if(t<0) t += 2*A*D;
			if(t<0 || t>1) return RayCast.FAIL;
			else return t;
		}else
			return RayCast.FAIL;
	}
	
	
	
	static private inline function rayPoly(r:Ray,p:Polygon,n:Vec2) {
		var ret = RayCast.FAIL;
		
		var pre:Vec2 = null;
		var ite = p.paxi.begin();
		var pax = p.paxi.front();
		var rneg:Bool = false;
		CxIter(cur,p.pverts,{
			if(pre!=null) {
				var neg:Bool;
				var t;  {
		vec_new(q); vec_sub(cur.p,pre.p,q);
		var denom = vec_cross(r.v,q);
		neg = denom<0;
		if(denom==0) t = RayCast.FAIL;
		else {
			denom = 1/denom;
			vec_new(ba); vec_sub(pre.p,r.a,ba);
			
			var t0 = vec_cross(ba,q)*denom;
			if(!(t0<0||t0>1)) {
				var s = vec_cross(ba,r.v)*denom;
				if(!(s<0||s>1)) t = t0;
				else t = RayCast.FAIL;
			}else t = RayCast.FAIL;
		}
	};
				if(t<ret) {
					ret = t;
					vec_set(n.p,pax.n);
					rneg = neg;
				}
			}
			pre = cur;
			pax = ite.elem();
			ite = ite.next;
		});
		
		var neg:Bool;
		var t;  {
		vec_new(q); vec_sub(p.pverts.front().p,pre.p,q);
		var denom = vec_cross(r.v,q);
		neg = denom<0;
		if(denom==0) t = RayCast.FAIL;
		else {
			denom = 1/denom;
			vec_new(ba); vec_sub(pre.p,r.a,ba);
			
			var t0 = vec_cross(ba,q)*denom;
			if(!(t0<0||t0>1)) {
				var s = vec_cross(ba,r.v)*denom;
				if(!(s<0||s>1)) t = t0;
				else t = RayCast.FAIL;
			}else t = RayCast.FAIL;
		}
	};
		if(t<ret) {
			ret = t;
			vec_set(n.p,pax.n);
			rneg = neg;
		}
		
		if(rneg) vec_negate(n.p);
		
		return ret;
	}
	
	static public inline function rayPolygon(r:Ray,p:Polygon,n:Vec2) {
		var aabb = p.aabb;
		if(aabb.contains2(r.ax,r.ay)) {
			return rayPoly(r,p,n);
		}else {
			var int = false;
			{
				if(r.vx>0) {
					var t = (aabb.minx-r.ax)/r.vx;
					if(!(t<0||t>1)) {
						var y = r.ay+r.vy*t;
						if(!(y<aabb.miny||y>aabb.maxy)) int = true;
					}
				}else if(r.vx<0) {
					var t = (aabb.maxx-r.ax)/r.vx;
					if(!(t<0||t>1)) {
						var y = r.ay+r.vy*t;
						if(!(y<aabb.miny||y>aabb.maxy)) int = true;
					}
				}
				
				if(!int) {
					if(r.vy>0) {
						var t = (aabb.miny-r.ay)/r.vy;
						if(!(t<0||t>1)) {
							var x = r.ax+r.vx*t;
							if(!(x<aabb.minx||x>aabb.maxx)) int = true;
						}
					}else if(r.vy<0) {
						var t = (aabb.maxy-r.ay)/r.vy;
						if(!(t<0||t>1)) {
							var x = r.ax+r.vx*t;
							if(!(x<aabb.minx||x>aabb.maxx)) int = true;
						}
					}
				}
			}
			
			if(int) {
				return rayPoly(r,p,n);
			}else
				return RayCast.FAIL;
		}
	}
}
